
function dxx = Bcr4bp_StmSB1Rot3D(t , xx , aux)
%
% BCR4BP动力学模型，状态+STM，SB1Rot，3D
%
% 输入：
% t:            [1x1]            时间(s)
% xx:          [42x1]            状态(km , km/s)
% aux:        [1x1]            引力常数
%
% 输出：
% dxx:        [42x1]            动力学方程(km , km/s^2)
%
% 参考：
% [1] 2022_Boudad
%
% 时间：2024年06月03日
% 作者：张晨
% 单位：CSU
% 邮箱：chenzhang@csu.ac.cn
%%%%%%%%%%%%%%%%%%%%%%%

% ---------------- 载入参数 ----------------
mu = aux.EMRot.mu;
mus = aux.EMRot.mus;
as = aux.EMRot.as;
wm = aux.SB1Rot.wm;

% ---------------- 提取变量 ----------------
x = xx(1);
y = xx(2);
z = xx(3);
vx = xx(4);
vy = xx(5);
vz = xx(6);

% ---------------- 天体位置 ----------------
% 地月系统相对SB1夹角
thetam = t * wm;

% B1位置
xb1 = mus / (mus + 1);
yb1 = 0;
zb1 = 0;

% B2位置
xb2 = 0;
yb2 = 0;
zb2 = 0;

% 地球位置
xe = xb1 - mu / as * cos(thetam);
ye = yb1 - mu / as * sin(thetam);
ze = 0;

% 月球位置
xm = xb1 + (1 - mu) / as * cos(thetam);
ym = yb1 + (1 - mu) / as * sin(thetam);
zm = 0;

% 太阳位置
xs = - 1 / (mus + 1);
ys = 0;
zs = 0;

% % --------------- E/M 碰撞检测 ---------------
% if aux.impact_IO == 1
%     
%     if sqrt((x - xe)^2 + (y - ye)^2 + (z - ze)^2) < aux.planet.req(3) / aux.dim.l_SB1
%         dxx = [];
%         return;
%     end
%     
%     if sqrt((x - xm)^2 + (y - ym)^2 + (z - zm)^2) < aux.planet.req(10) / aux.dim.l_SB1
%         dxx = [];
%         return;
%     end
%     
% end

% ----------------- SB1rot动力学方程 ----------------
r_ec_3pow = ((x - xe)^2 + (y - ye)^2 + (z - ze)^2)^(1.5);
r_mc_3pow = ((x - xm)^2 + (y - ym)^2 + (z - zm)^2)^(1.5);
r_sc_3pow = ((x - xs)^2 + (y - ys)^2 + (z - zs)^2)^(1.5);

r_ec_5pow = r_ec_3pow^(5/3);
r_mc_5pow = r_mc_3pow^(5/3);
r_sc_5pow = r_sc_3pow^(5/3);

% -----------------------------------------------------------
dax_dx = - (1 - mu) / (mus + 1) * (1 / r_ec_3pow - 3 * (x - xe) * (x - xe) / r_ec_5pow) ...
                        - mu / (mus + 1) * (1 / r_mc_3pow - 3 * (x - xm) * (x - xm) / r_mc_5pow) ...
                      - mus / (mus + 1) * (1 / r_sc_3pow - 3 * (x - xs) * (x - xs) / r_sc_5pow) + 1;

dax_dy = - (1 - mu) / (mus + 1) * ( - 3 * (x - xe) * (y - ye) / r_ec_5pow) ...
                        - mu / (mus + 1) * ( - 3 * (x - xm) * (y - ym) / r_mc_5pow) ...
                      - mus / (mus + 1) * ( - 3 * (x - xs) * (y - ys) / r_sc_5pow);

dax_dz = - (1 - mu) / (mus + 1) * ( - 3 * (x - xe) * (z - ye) / r_ec_5pow) ...
                        - mu / (mus + 1) * ( - 3 * (x - xm) * (z - zm) / r_mc_5pow) ...
                      - mus / (mus + 1) * ( - 3 * (x - xs) * (z - zs) / r_sc_5pow);

% day_dx = - (1 - mu) / (mus + 1) * ( - 3 * (y - ye) * (x - xe) / r_ec_5pow) ...
%                         - mu / (mus + 1) * ( - 3 * (y - ym) * (x - xm) / r_mc_5pow) ...
%                       - mus / (mus + 1) * ( - 3 * (y - ys) * (x - xs) / r_sc_5pow);
day_dx = dax_dy;

day_dy = - (1 - mu) / (mus + 1) * (1 / r_ec_3pow - 3 * (y - ye) * (y - ye) / r_ec_5pow) ...
                        - mu / (mus + 1) * (1 / r_mc_3pow - 3 * (y - ym) * (y - ym) / r_mc_5pow) ...
                      - mus / (mus + 1) * (1 / r_sc_3pow - 3 * (y - ys) * (y - ys) / r_sc_5pow) + 1;

day_dz = - (1 - mu) / (mus + 1) * ( - 3 * (y - ye) * (z - ze) / r_ec_5pow) ...
                        - mu / (mus + 1) * ( - 3 * (y - ym) * (z - zm) / r_mc_5pow) ...
                      - mus / (mus + 1) * ( - 3 * (y - ys) * (z - zs) / r_sc_5pow);

% daz_dx = - (1 - mu) / (mus + 1) * ( - 3 * (z - ze) * (x - xe) / r_ec_5pow) ...
%                         - mu / (mus + 1) * ( - 3 * (z - zm) * (x - xm) / r_mc_5pow) ...
%                       - mus / (mus + 1) * ( - 3 * (z - zs) * (x - xs) / r_sc_5pow);
daz_dx = dax_dz;

% daz_dy = - (1 - mu) / (mus + 1) * ( - 3 * (z - ze) * (y - ye) / r_ec_5pow) ...
%                         - mu / (mus + 1) * ( - 3 * (z - zm) * (y - ym) / r_mc_5pow) ...
%                        - mus / (mus + 1) * ( - 3 * (z - zs) * (y - ys) / r_sc_5pow);
daz_dy = day_dz;

daz_dz = - (1 - mu) / (mus + 1) * (1 / r_ec_3pow - 3 * (z - ze) * (z - ze) / r_ec_5pow) ...
                        - mu / (mus + 1) * (1 / r_mc_3pow - 3 * (z - zm) * (z - zm) / r_mc_5pow) ...
                      - mus / (mus + 1) * (1 / r_sc_3pow - 3 * (z - zs) * (z - zs) / r_sc_5pow);

Df = [0 , 0 , 0 , 1 , 0 , 0;
         0 , 0 , 0 , 0 , 1 , 0;
         0 , 0 , 0 , 0 , 0 , 1;
        dax_dx , dax_dy , dax_dz ,  0 , 2 , 0;
        day_dx , day_dy , day_dz , -2 , 0 , 0;
        daz_dx , daz_dy , daz_dz ,  0 , 0 , 0];

Phi = reshape(xx(7 : 42) , 6 , 6);
dPhi = Df * Phi;

dxx = zeros(42 , 1);
dxx(1 : 6) = [vx;
    vy;
    vz;
    x + 2 * vy - (1 - mu) / (mus + 1) * (x - xe) / r_ec_3pow - mu / (mus + 1) * (x - xm) / r_mc_3pow - mus / (mus + 1) * (x - xs) / r_sc_3pow;
    y - 2 * vx - (1 - mu) / (mus + 1) * (y - ye) / r_ec_3pow - mu / (mus + 1) * (y - ym) / r_mc_3pow - mus / (mus + 1) * (y - ys) / r_sc_3pow;
    - (1 - mu) / (mus + 1)* (z - ze) / r_ec_3pow - mu / (mus + 1) * (z - zm) / r_mc_3pow - mus / (mus + 1) * (z - zs) / r_sc_3pow];
dxx(7 : 42) = dPhi(:);

end
